package behavior;

import lejos.nxt.TouchSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;

/**
 * This behavior is activated when the robot hits a wall on the right.
 * The robot will drive backwards, stop and turn to the left.
 */
public class RightBumper implements Behavior{
	
	
	private TouchSensor touchSensor;
	private TachoPilot pilot;
	private boolean isSuppressed = false;

	public RightBumper(TouchSensor touchSensor, TachoPilot pilot){
		this.touchSensor = touchSensor;
		this.pilot = pilot;
	}
	
	@Override
	public void action() {
		isSuppressed = false;
		//INCHES
		pilot.travel(-2);
		pilot.rotate(25);
		
		while(pilot.isMoving() && !isSuppressed)
		{
			Thread.yield();
		}
		pilot.stop();
	}

	@Override
	public void suppress() {
		isSuppressed = true;		
	}

	@Override
	public boolean takeControl() {
		
		return touchSensor.isPressed();
	}

}
